#include <raw_api.h>
#include <mm/pool/raw_memp.h>
#include <mm/pool/raw_mem.h>
#include <raw_debug.h>


/* lwIP head implemented with different sized pools */

/**
 * Allocate memory: determine the smallest pool that is big enough
 * to contain an element of 'size' and get an element from that pool.
 *
 * @param size the size in bytes of the memory needed
 * @return a pointer to the allocated memory or 0 if the pool is empty
 */
void *
mem_malloc(mem_size_t size)
{
  void *ret;
  struct memp_malloc_helper *element;
  memp_t poolnr;
  mem_size_t required_size = size + LWIP_MEM_ALIGN_SIZE(sizeof(struct memp_malloc_helper));

  for (poolnr = MEMP_POOL_FIRST; poolnr <= MEMP_POOL_LAST; poolnr = (memp_t)(poolnr + 1)) {
#if MEM_USE_POOLS_TRY_BIGGER_POOL
again:
#endif /* MEM_USE_POOLS_TRY_BIGGER_POOL */
    /* is this pool big enough to hold an element of the required size
       plus a struct memp_malloc_helper that saves the pool this element came from? */
    if (required_size <= memp_pools[poolnr]->size) {
      break;
    }
  }
  if (poolnr > MEMP_POOL_LAST) {
    RAW_MESSAGE_ASSERT("mem_malloc(): no pool is that big!", 0);
    return 0;
  }
  element = (struct memp_malloc_helper*)memp_malloc(poolnr);
  if (element == 0) {
    /* No need to DEBUGF or ASSERT: This error is already
       taken care of in memp.c */
#if MEM_USE_POOLS_TRY_BIGGER_POOL
    /** Try a bigger pool if this one is empty! */
    if (poolnr < MEMP_POOL_LAST) {
      poolnr++;
      goto again;
    }
#endif /* MEM_USE_POOLS_TRY_BIGGER_POOL */
    return 0;
  }

  /* save the pool number this element came from */
  element->poolnr = poolnr;
  /* and return a pointer to the memory directly after the struct memp_malloc_helper */
  ret = (RAW_U8*)element + LWIP_MEM_ALIGN_SIZE(sizeof(struct memp_malloc_helper));

#if MEMP_OVERFLOW_CHECK
  /* initialize unused memory */
  element->size = size;
  raw_memset((RAW_U8 *)ret + size, 0xcd, memp_pools[poolnr]->size - size);
#endif /* MEMP_OVERFLOW_CHECK */
  return ret;
}

/**
 * Free memory previously allocated by mem_malloc. Loads the pool number
 * and calls memp_free with that pool number to put the element back into
 * its pool
 *
 * @param rmem the memory element to free
 */
void
mem_free(void *rmem)
{
  struct memp_malloc_helper *hmem;

  RAW_MESSAGE_ASSERT("rmem != 0", (rmem != 0));
  RAW_MESSAGE_ASSERT("rmem == MEM_ALIGN(rmem)", (rmem == LWIP_MEM_ALIGN(rmem)));

  /* get the original struct memp_malloc_helper */
  /* cast through void* to get rid of alignment warnings */
  hmem = (struct memp_malloc_helper*)(void*)((RAW_U8*)rmem - LWIP_MEM_ALIGN_SIZE(sizeof(struct memp_malloc_helper)));

  RAW_MESSAGE_ASSERT("hmem != 0", (hmem != 0));
  RAW_MESSAGE_ASSERT("hmem == MEM_ALIGN(hmem)", (hmem == LWIP_MEM_ALIGN(hmem)));
  RAW_MESSAGE_ASSERT("hmem->poolnr < MEMP_MAX", (hmem->poolnr < MEMP_MAX));

#if MEMP_OVERFLOW_CHECK
  {
     RAW_U16 i;
     RAW_MESSAGE_ASSERT("MEM_USE_POOLS: invalid chunk size",
        hmem->size <= memp_pools[hmem->poolnr]->size);
     /* check that unused memory remained untouched */
     for (i = hmem->size; i < memp_pools[hmem->poolnr]->size; i++) {
        RAW_U8 data = *((RAW_U8*)rmem + i);
        RAW_MESSAGE_ASSERT("MEM_USE_POOLS: mem overflow detected", data == 0xcd);
     }
  }
#endif /* MEMP_OVERFLOW_CHECK */

  /* and put it in the pool we saved earlier */
  memp_free(hmem->poolnr, hmem);
}

